/*
 * Copyright (c) 1997, 1998, 2003, 2006 Aleksandar Samardzic
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <assert.h>		/* for assert() function */
#include <stdlib.h>		/* for malloc() function */
#include "bounding_volume_hierarchy.h"

typedef struct _PIntersectedNode {
	PTreeNode      *pNode;
	double          t;
} PIntersectedNode;

/* IntersectedNode_IsCloser - determine which bounding volume hierearchy
 * node is closer */
int
IntersectedNode_IsCloser(void *pFirst, void *pSecond)
{
	return ((PIntersectedNode *) pFirst)->t <
	    ((PIntersectedNode *) pSecond)->t;
}

/* BoundingVolumeHierarchy_Init - BoundingVolume class constructor */
void
BoundingVolumeHierarchy_Init(this, pSlabs, pPrimitives, seed)
     PBoundingVolumeHierarchy *this;
     PSlabs         *pSlabs;
     PList          *pPrimitives;
     int             seed;
{
	PPrimitive     *pPrimitive;
	PBoundingVolume *pCurrVolume,
	               *pLocalVolume,
	               *pNewVolume;
	PTree          *pTree;
	PTreeNode      *pTreeNode;
	PTreeUpIterator upIt;
	PList           list;
	PListIterator   listIt,
	                listIt2;
	PStack          stack;
	PTreeNode      *pBestNode,
	               *pLocalNode;
	double          bestCost,
	                inhCost,
	                localCost[2];
	PAddMode        bestAddMode,
	                localAddMode;
	double          localNewArea;
	double          bestIncCost,
	                localIncCost;
	PBoundingVolume tmpVolume;
	PBoundingVolume *pBoundingVolume;

	/* initialize BoundingVolume data */
	this->slabs.numSlabs = pSlabs->numSlabs;
	this->slabs.pVectors =
	    malloc(this->slabs.numSlabs * sizeof(PVector));
	assert(this->slabs.pVectors != NULL);
	memcpy(this->slabs.pVectors, pSlabs->pVectors,
	       this->slabs.numSlabs * sizeof(PVector));

	this->numPrimitives = pPrimitives->numItems;
	Tree_Init(&(this->tree));

	/* shuffle primitives */
	List_Shuffle(pPrimitives, seed);

	/* calculate bounding volume for each primitive and store mutual
	 * references */
	for (ListIterator_Attach(&listIt, pPrimitives);
	     ListIterator_Next(&listIt, (void **) &pPrimitive);) {
		pNewVolume = malloc(sizeof(PBoundingVolume));
		assert(pNewVolume != NULL);
		BoundingVolume_Init(pNewVolume, &(this->slabs));
		pPrimitive->CalcBoundingVolume(pPrimitive, pNewVolume,
					       &(this->slabs));
		BoundingVolume_CalcArea(pNewVolume);
		pNewVolume->pPrimitive = pPrimitive;
		pPrimitive->pBoundingVolume = pNewVolume;
	}

	/* automatic generation of bounding volume hierarchy according to
	 * [Gold87] */
	BoundingVolume_Init(&tmpVolume, &(this->slabs));
	List_Init(&list);
	Stack_Init(&stack);
	pTree = &(this->tree);

	for (ListIterator_Attach(&listIt, pPrimitives);
	     ListIterator_Next(&listIt, (void **) &pPrimitive);) {
		/* for each primitive */
		if (pTree->pRoot == NULL)	/* first primitive */
			Tree_AddItem(pTree, pPrimitive->pBoundingVolume,
				     NULL, CHILD);
		else {		/* rest of primitives */

			pCurrVolume = pPrimitive->pBoundingVolume;

			/* bounding volume's data entry t will here be
			 * used to store inherited cost */
			((PBoundingVolume *) pTree->pRoot->pItem)->cost =
			    0;

			/* initialize stack of candidate nodes for
			 * insertion */
			Stack_Push(&stack, pTree->pRoot);
			bestCost = DBL_MAX, pBestNode = NULL;

			for (; pLocalNode = Stack_Pop(&stack);) {	/* while 
									 * there 
									 * are 
									 * nodes 
									 * on 
									 * stack 
									 */
				/* initialize inherited cost and best
				 * incremental cost */
				inhCost =
				    ((PBoundingVolume *) pLocalNode->
				     pItem)->cost;
				bestIncCost = DBL_MAX;

				/* empty next step candidate node list */
				List_Empty(&list);

				for (; pLocalNode; pLocalNode = pLocalNode->pNext) {	/* for 
											 * each 
											 * node 
											 * at 
											 * the 
											 * current 
											 * depth 
											 * level 
											 */
					pLocalVolume = pLocalNode->pItem;

					/* calculate increase of area of
					 * local node's bounding volume
					 * when combined with new node */
					BoundingVolume_Combine(&tmpVolume,
							       pCurrVolume,
							       pLocalVolume);
					BoundingVolume_CalcArea
					    (&tmpVolume);
					localNewArea = tmpVolume.area;

					/* calculate best local insertion
					 * costs and remember better of
					 * two local insertion methods */
					if (pLocalNode->numChildren) {
						localCost[SIBLING] =
						    2 * localNewArea;
						localCost[CHILD] =
						    pLocalNode->
						    numChildren *
						    (localNewArea -
						     pLocalVolume->area) +
						    localNewArea;
						localAddMode =
						    (localCost[SIBLING] <
						     localCost[CHILD]) ?
						    SIBLING : CHILD;
					} else {
						localCost[SIBLING] =
						    2 * localNewArea;
						localAddMode = SIBLING;
					}

					/* if local node offers smallest
					 * cost so far, remember it */
					if (localCost[localAddMode] +
					    inhCost < bestCost) {
						bestCost =
						    localCost[localAddMode]
						    + inhCost;
						pBestNode = pLocalNode;
						bestAddMode = localAddMode;
					}

					if (pLocalNode->numChildren) {
						/* adjust next step
						 * candidate list */
						localIncCost =
						    pLocalNode->
						    numChildren *
						    (localNewArea -
						     pLocalVolume->area);
						if (localIncCost <
						    bestIncCost) {
							bestIncCost =
							    localIncCost;
							List_Empty(&list);
							List_AddItem(&list,
								     pLocalNode);
						} else if (localIncCost ==
							   bestIncCost)
							List_AddItem(&list,
								     pLocalNode);
					}
				}

				/* if appropriate, push candidates at the
				 * stack */
				if ((inhCost += bestIncCost) < bestCost)
					for (ListIterator_Attach
					     (&listIt2, &list);
					     ListIterator_Next(&listIt2,
							       (void **)
							       &pLocalNode);) {
						((PBoundingVolume *)
						 pLocalNode->pFirstChild->
						 pItem)->cost = inhCost;
						Stack_Push(&stack,
							   pLocalNode->
							   pFirstChild);
					}
			}

			/* insert node node into the tree, according to
			 * best insertion node and best insertion method
			 * determined */
			pTreeNode =
			    Tree_AddItem(pTree, pCurrVolume, pBestNode,
					 bestAddMode);

			/* if necessary, create bounding volume for new
			 * parent node */
			if (bestAddMode == SIBLING) {
				pNewVolume =
				    malloc(sizeof(PBoundingVolume));
				assert(pNewVolume != NULL);
				BoundingVolume_Init(pNewVolume,
						    &(this->slabs));
				BoundingVolume_Combine(pNewVolume,
						       pBestNode->
						       pFirstChild->pItem,
						       pBestNode->
						       pLastChild->pItem);
				BoundingVolume_CalcArea(pNewVolume);
				pBestNode->pItem = pNewVolume;
			}

			/* adjust bounding volumes for ancestors of new
			 * node */
			for (TreeUpIterator_Attach
			     (&upIt, pTreeNode->pParent);
			     TreeUpIterator_Next(&upIt, (void **)
						 &pBoundingVolume);) {
				BoundingVolume_Combine(pBoundingVolume,
						       pBoundingVolume,
						       pCurrVolume);
				BoundingVolume_CalcArea(pBoundingVolume);
			}
		}
	}

	/* clean up structures used in algorithm */
	BoundingVolume_Clean(&tmpVolume);
	Stack_Clean(&stack);
	List_Clean(&list);
}

/* BoundingVolumeHierarchy_Clean - BoundingVolume class destructor */
void
BoundingVolumeHierarchy_Clean(this)
     PBoundingVolumeHierarchy *this;
{
	PTreePreorderIterator treeIt;
	PBoundingVolume *pBoundingVolume;

	free(this->slabs.pVectors);

	for (TreePreorderIterator_Attach(&treeIt, &(this->tree));
	     TreePreorderIterator_Next(&treeIt,
				       (void **) &pBoundingVolume);) {
		BoundingVolume_Clean(pBoundingVolume);
		free(pBoundingVolume);
	}

	Tree_Clean(&(this->tree));
}

/* BoundingVolumeHierarchy_Intersect - find intersection of bounding
 * volume hierearchy with ray according to [Kay86] */
PPrimitive     *
BoundingVolumeHierarchy_Intersect(this, pRay)
     PBoundingVolumeHierarchy *this;
     PRay           *pRay;
{
	PPrimitive     *pIntersectedPrimitive = NULL;
	double         *p0 = pRay->p0,
	    *dp = pRay->dp;
	Byte            slab,
	                numSlabs = this->slabs.numSlabs;
	PVector        *pSlabVectors = this->slabs.pVectors;
	double         *S,
	               *T;
	double          denom;
	PHeap           heap;
	PTreeNode      *pCurrNode;
	PBoundingVolume *pBoundingVolume;
	PPrimitive     *pPrimitive;
	double          t;
	PIntersectedNode *pIntersectedNode;

	if (!(pCurrNode = this->tree.pRoot))
		return FALSE;

	/* precompute all values that are constants for a ray */
	S = malloc(numSlabs * sizeof(double));
	assert(S != NULL);
	T = malloc(numSlabs * sizeof(double));
	assert(T != NULL);
	for (slab = 0; slab < numSlabs; slab++) {
		S[slab] = Vector_DotProduct(pSlabVectors[slab], p0);
		T[slab] = (denom =
			   Vector_DotProduct(pSlabVectors[slab],
					     dp)) ? 1 / denom : DBL_MAX;
	}

	/* initialize heap */
	Heap_Init(&heap, this->numPrimitives + 1, 0,
		  IntersectedNode_IsCloser);

	/* intersect ray with hierearchy */
	FOREVER {
		/* for each node at current tree level */
		for (; pCurrNode; pCurrNode = pCurrNode->pNext) {
			pBoundingVolume = pCurrNode->pItem;
			if (pPrimitive = pBoundingVolume->pPrimitive) {	/* if 
									 * leaf 
									 */
				/* intersect primitive with ray */
				if (pPrimitive->
				    Intersect(pPrimitive, (PLine *) pRay))
					pIntersectedPrimitive = pPrimitive;
			} else {
				/* intersect current bounding volume with
				 * ray */
				if ((t = BoundingVolume_Intersect(pBoundingVolume, S, T)) == DBL_MAX)	/* no 
													 * intersection */
					continue;

				/* push current node at heap */
				pIntersectedNode =
				    malloc(sizeof(PIntersectedNode));
				assert(pIntersectedNode != NULL);
				pIntersectedNode->pNode = pCurrNode;
				pIntersectedNode->t = t;
				Heap_Push(&heap, pIntersectedNode);
			}
		}

		/* check for break conditions */
		if (Heap_IsEmpty(&heap)
		    || ((PIntersectedNode *) Heap_First(&heap))->t >=
		    pRay->t)
			break;

		/* promote child of first node from heap to current node */
		pIntersectedNode = Heap_Pop(&heap);
		pCurrNode = pIntersectedNode->pNode->pFirstChild;
		free(pIntersectedNode);
	}

	/* clean up */
	FOREVER {
		if (Heap_IsEmpty(&heap))
			break;
		free(Heap_Pop(&heap));
	}
	Heap_Clean(&heap);
	free(S);
	free(T);
	return pIntersectedPrimitive;
}
